#include <iostream>
#include <math.h>
#include "waypoint.h"
using namespace std;

WayPoint::WayPoint()
{
}

WayPoint::~WayPoint()
{
}

void WayPoint::setX(double x)
{
	x_=x;
}

double WayPoint::getX()
{
	return x_;
}

void WayPoint::setY(double y)
{
	y_=y;
}

double WayPoint::getY()
{
	return y_;
}

void WayPoint::setTheta(double theta)
{
	theta_=theta;
}

double WayPoint::getTheta()
{
	return theta_;
}

void WayPoint::setTimeStamp(double timestamp)
{
	timestamp_=timestamp;
}

double WayPoint::getTimeStamp()
{
	return timestamp_;
}

void WayPoint::setKick(bool kick)
{
	kick_=kick;
}

bool WayPoint::getKick()
{
	return kick_;
}

double WayPoint::computeDistance(WayPoint wp)
{
	return sqrt((getX()-wp.getX())*(getX()-wp.getX()) + (getY()-wp.getY())*(getY()-wp.getY()));
}


void WayPoint::Properties()
{
	cout << "\nCharacteristics of this WayPoint";
	cout << "\nx   = " << getX();
	cout << "\ny   = " << getY();
	cout << "\ntheta = " << getTheta();
	cout << "\ntimestamp = " << getTimeStamp();
	cout << "\nkick = " << getKick() << endl;
}
